package ai4games_tests.maths;

import game2dai.maths.FastMath;
import game2dai.maths.Geometry2D;
import game2dai.maths.Vector2D;
import processing.core.PApplet;

public class NoOverlap extends PApplet {


	public Thing mover, other;
	public float sensitivity = 25;
	public boolean isdragging = false;
	public int forceCol, colNormalCol;
	public Vector2D collNorm = new Vector2D();;
	public Vector2D vN = new Vector2D();
	public Vector2D relVel = new Vector2D();
	public Vector2D resolvedForce = new Vector2D();

	float nv, n2;
	public float arrowLength = 16;
	public float arrowWidth = 10;

	public boolean movingApart = false;

	public void setup(){
		size(600,600);
		cursor(CROSS);
		ellipseMode(CENTER);
		rectMode(CENTER);
		forceCol = color(32,32,255);
		colNormalCol = color(255,255,0);
		mover = new Thing(new Vector2D(width/2, height/2), new Vector2D(40,-22), color(32,255,32), color(255,255,160));
		other = new Thing(new Vector2D(width/3, height/5), new Vector2D(50,75), color(255,32,32), color(255,255,160));
		calcResolvedForce();
	}


	public void draw(){	
		background(0);
		drawForce();
		drawResolvedForce();
		mover.draw();
		other.draw();
		drawStatus();
	}


	public void calcResolvedForce(){
		collNorm.set(mover.pos.x - other.pos.x, mover.pos.y - other.pos.y);
		relVel.set(mover.vel.x - other.vel.x, mover.vel.y - other.vel.y);
		nv = (float) collNorm.dot(relVel);
		n2 = (float) collNorm.dot(collNorm);
		System.out.println("nv= " + nv + "    n2= " + n2);
		movingApart = (nv > 0 || n2 < 1E-5);
		if(!movingApart){
			float f = nv/n2;
			vN.set(-collNorm.x * f, -collNorm.y * f);
		}
	}
	
	public void drawStatus(){
		float cosang, angRad, ang;
		int x, y;
		fill(255);
		noStroke();

		x = 20;
		y = 30;
		fill(mover.thingCol);
		text("Mover ", x, y);
		fill(255);
		text(" <> ", x + 50, y);
		fill(other.thingCol);
		text("Other ", x + 90, y);
		fill(255);
		cosang = (float) mover.vel.dotNorm(other.vel);
		angRad = (float) FastMath.acos(cosang);
		ang = angRad * 180.0f / PI;
		text(""+cosang, x + 200,y);
		text(""+angRad, x + 320,y);
		text(""+ang, x + 440,y);

		y = 50;
		fill(mover.thingCol);
		text("Mover ", x, y);
		fill(255);
		text(" <> ", x + 50, y);
		fill(forceCol);
		text("Force ", x + 90, y);
		fill(255);
		//		text("Angle between mover and force", x, y);
		cosang = (float) mover.vel.dotNorm(collNorm);
		angRad = (float) FastMath.acos(cosang);
		ang = angRad * 180.0f / PI;
		text(""+cosang, x + 200,y);
		text(""+angRad, x + 320,y);
		text(""+ang, x + 440,y);

		y = 70;
		fill(other.thingCol);
		text("Other ", x, y);
		fill(255);
		text(" <> ", x + 50, y);
		fill(forceCol);
		text("Force ", x + 90, y);
		fill(255);
		//		text("Angle between other and force", x, y);
		cosang = (float) other.vel.dotNorm(collNorm);
		angRad = (float) FastMath.acos(cosang);
		ang = angRad * 180.0f / PI;
		text(""+cosang, x + 200,y);
		text(""+angRad, x + 320,y);
		text(""+ang, x + 440,y);

		fill(255);
		y = 100;
		if(movingApart)
			text("MOVING APART", x, y);
		else
			text("MOVING TOGETHER", x, y);

	}

	public void drawResolvedForce(){
		if(!movingApart){
			stroke(colNormalCol);
			fill(colNormalCol);
			strokeWeight(1.5f);
			pushMatrix();
			translate((float)mover.pos.x, (float)mover.pos.y);
			float angle = (float) FastMath.atan2(vN.y, vN.x);
			float forceSize = (float) vN.length();
			rotate(angle);
			line(0,0,forceSize - arrowLength, 0);
			noStroke();
			beginShape(TRIANGLES);
			vertex(forceSize - arrowLength, - arrowWidth/2);
			vertex(forceSize - arrowLength, + arrowWidth/2);
			vertex(forceSize, 0);
			endShape(CLOSE);
			popMatrix();
		}
	}

	public void drawForce(){
		stroke(forceCol);
		fill(forceCol);
		strokeWeight(20);
		pushMatrix();
		translate((float)other.pos.x, (float)other.pos.y);
		float angle = (float) FastMath.atan2(collNorm.y, collNorm.x);
		float forceSize = (float) collNorm.length();
		rotate(angle);
		line(0,0,forceSize - 60, 0);
		noStroke();
		beginShape(TRIANGLES);
		vertex(forceSize - 60, - 40/2);
		vertex(forceSize - 60, + 40/2);
		vertex(forceSize, 0);
		endShape(CLOSE);
		popMatrix();
	}

	public void mouseMoved(){
		if(!isdragging){
			mover.isOver(mouseX, mouseY);
			other.isOver(mouseX, mouseY);
		}
	}

	public void mouseReleased(){
		isdragging = false;
	}

	public void mouseDragged(){
		float dx = mouseX - pmouseX;
		float dy = mouseY - pmouseY;
		mover.change(dx, dy);
		other.change(dx, dy);
		calcResolvedForce();
	}




	class Thing {
		public Vector2D pos = new Vector2D();
		public Vector2D vel = new Vector2D();

		public float strokeWeight = 1.5f;
		public int thingCol = color(0);
		public int selCol = color(255);
		public int mode = 0;

		public int size = 12;


		/**
		 * @param pos
		 * @param vel
		 * @param thingCol
		 * @param selCol
		 */
		public Thing(Vector2D pos, Vector2D vel, int thingCol, int highlightCol) {
			this.pos.set(pos);
			this.vel.set(vel);
			this.thingCol = thingCol;
			this.selCol = highlightCol;
		}

		public int isOver(float mx, float my){
			mode = 0;
			if(Geometry2D.distance_sq(pos.x + vel.x, pos.y + vel.y, mx, my) < sensitivity)
				mode = 1;
			else if(Geometry2D.distance_sq(pos.x, pos.y, mx, my) < sensitivity)
				mode = 2;
			return mode;
		}

		public void change(float dx, float dy){
			switch(mode){
			case 1:
				vel.add(dx, dy);
				break;
			case 2:
				pos.add(dx, dy);
				break;
			}

		}

		public void draw(){
			float angle = (float) FastMath.atan2(vel.y, vel.x);
			float speed = (float) vel.length();
			pushMatrix();
			translate((float)pos.x, (float)pos.y);
			rotate(angle);
			strokeWeight(strokeWeight);
			stroke(thingCol);
			line(0,0, speed, 0);
			fill(thingCol);
			noStroke();
			beginShape(TRIANGLES);
			vertex(speed - arrowLength, - arrowWidth/2);
			vertex(speed - arrowLength, + arrowWidth/2);
			vertex(speed, 0);
			endShape(CLOSE);
			ellipse(0,0, size, size);

			noFill();
			strokeWeight(2);
			stroke(selCol);
			switch(mode){
			case 1:
				ellipse(speed, 0, 16, 16);
				break;
			case 2:
				ellipse(0, 0, 16, 16);
				break;
			}

			popMatrix();
		}


	}
}
